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measuring an angular velocity of the sensor r*~ 



integrating angular rate values 



normalizing the integrated angular rate values 
to produce an estimate of sensor orientation 
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measuring a magnetic field vector 
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measuring a gravity vector 
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determining a measurement vector 
from magnetic field vector gravity vector 
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calculating a computed measurement vector 
from estimated sensor orientation 
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generating an error vector 
that defines a criterion function 



minimizing the criterion function to 
output an orientation error estimate 



integrating the orientation error estimate 



normalizing the integrated error estimate 
to produce a new estimate of sensor orientation 
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using the new estimate of sensor orientation to 
calculate computed measurement vector 513 and 
repeating steps 501-523 
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providing a starting estimate 
of sensor orientation quaternion 



measuring a magnetic field vector 



measuring a gravity vector 
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determining a measurement vector 
from the magnetic field vector and the gravity vector 



calculating a computed measurement vector 
using quaternion mathematics 



generating an 6xt error vector 
that defines a criterion function 



minimizing the criterion function 
and outputting a 4x1 quaternion error estimate 
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integrating the quaternion error estimate 
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normalizing the integrated quaternion error estimate 
to produce a new estimated sensor orientation quaternion 
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calculating a computed measurement vector 609 
using the new estimated sensor orientation quaternion and I 
repeating steps 6 Oj -6 19 



